(load "unittest.l")
(load "package://pr2eus/make-pr2-model-file")
(ros::roseus "make-pr2-modle-file")
(make-pr2-model-file :output-directory "/tmp")
(load "/tmp/pr2.l")
(load "package://pr2eus/pr2-interface.l")

(init-unit-test)

;; test for pr2-interface
;;  . read topics which is subscribed by pr2-interface

(ros::roseus "pr2-read-state-test")
(pr2)
(defvar *ri* (instance pr2-interface :init))
(defvar *tfl* (instance ros::transform-listener :init))
(unix::sleep 1)

(deftest pr2-read-joint-state-test ()
  (ros::spin-once)
  (assert (send *ri* :state :stamp)
	  "can not read :stamp in robot-state")
  (assert (send *ri* :state :potentio-vector)
	  "can not read :potentio-vector")
  (assert (send *ri* :state :torque-vector)
	  "can not read torque-vector")
  (assert (send *ri* :state :stamp :wait-until-update nil)
	  "can not read :stamp :wait-until-update nil")
  (assert (send *ri* :state :potentio-vector :wait-until-update nil)
	  "can not read :potentio-vector :wait-until-update nil")
  (assert (send *ri* :state :torque-vector :wait-until-update nil)
	  "can not read torque-vector :wait-until-update nil")

  (let ((lend (send *tfl* :lookup-transform "/base_footprint" "/l_gripper_tool_frame"
		    (ros::time 0)))
	(rend (send *tfl* :lookup-transform "/base_footprint" "/r_gripper_tool_frame"
		    (ros::time 0))))
    (send *pr2* :angle-vector (send *ri* :state :potentio-vector))
    (dolist (link (send *pr2* :links))
      (assert (> 10 (distance (send (send *tfl* :lookup-transform "/base_footprint" (string-downcase (send link :name)) (ros::time 0)) :worldpos)
			      (send (send *pr2* :link (send link :name)) :worldpos)))
	      (format nil " ~A of model and one from tf are not same position" (send link :name)))
    )


    (assert (> 1 (distance (send *ri* :state :potentio-vector)
			   (send *pr2* :angle-vector)))
	    (format nil "angle-vector of model and one from joint-state are not same angle ~A ~A" (send *ri* :state :potentio-vector) (send *pr2* :angle-vector)))

    (assert (> 10 (distance (send lend :worldpos)
			    (send *pr2* :larm :end-coords :worldpos)))
	    (format nil "larm-end-coords of model and one from tf are not same position ~A ~A" (send lend :worldpos) (send *pr2* :larm :end-coords :worldpos)))
    (assert (> 10 (distance (send rend :worldpos)
			    (send *pr2* :rarm :end-coords :worldpos)))
	    (format nil "rarm-end-coords of model and one from tf are not same position ~A ~A" (send rend :worldpos) (send *pr2* :rarm :end-coords :worldpos)))
    )
  )

(deftest pr2-read-finger-pressure-test ()
  (ros::spin-once)
  (let ((larm-p (send *ri* :state :larm-pressure))
	(rarm-p (send *ri* :state :rarm-pressure)))
    (assert (= 22 (length (car  larm-p))))
    (assert (= 22 (length (cadr larm-p))))
    (assert (= 22 (length (car  rarm-p))))
    (assert (= 22 (length (cadr rarm-p))))
  ))

(deftest pr2-read-odom-test ()
  (ros::spin-once)
  (assert (send *ri* :state :odom :stamp))
  (assert (send *ri* :state :odom :pose))
  (assert (derivedp (send *ri* :state :odom :pose) coordinates))
  (assert (send *ri* :state :odom :velocity))
  (assert (send *ri* :state :odom :stamp :wait-until-update nil))
  (assert (send *ri* :state :odom :pose :wait-until-update nil))
  (assert (derivedp (send *ri* :state :odom :pose :wait-until-update nil)
                    coordinates))
  (assert (send *ri* :state :odom :velocity :wait-until-update nil))
  )

(deftest pr2-calc-worldcoords-test ()
  (ros::spin-once)
  (assert (send *ri* :state :worldcoords)
	  "failed to get robot position from /world frame")
  (assert (send *ri* :state :worldcoords :wait-until-update nil)
	  "failed to get robot position from /world frame")
  )

#|
(deftest pr2-link-coords-test ()
  (ros::spin-once)
  (send *pr2* :angle-vector (send *ri* :state :potentio-vector))
  (dolist (m (send *pr2* :methods))
    (when (substringp "_LINK" (string m))
    (let* ((frame-id (string-downcase (string m)))
	   (c (send *pr2* m))
	   (p (send *tfl* :lookup-transform "/base_footprint" frame-id (ros::time 0))))
      (assert (eps= (norm (send p :difference-position c)) 0.0 3.0)
	      (format nil "check ~A : ~A ~A, diff ~A" frame-id p c (norm (send p :difference-position c))))
      (assert (eps= (norm (send p :difference-rotation c)) 0.0 3.0)
	      (format nil "check ~A : ~A ~A, diff ~A" frame-id p c (norm (send p :difference-rotation c))))
      ))))
|#

(deftest pr2-camera-coords-test ()
  (ros::spin-once)
  (send *pr2* :angle-vector (send *ri* :state :potentio-vector))
  (dolist (camera (send *pr2* :cameras))
    (let* ((frame-id (string-downcase (string (send (send camera :parent) :name))))
	   (p (send *tfl* :lookup-transform "/base_footprint" frame-id (ros::time 0)))
	   (viewing (send camera :viewing)))
      (assert (eps= (norm (send p :difference-position viewing)) 0.0 3.0)
	      (format nil "check ~A : ~A ~A, diff ~A" frame-id p viewing (norm (send p :difference-position viewing))))
      (assert (eps= (norm (send p :difference-rotation viewing)) 0.0 3.0)
	      (format nil "check ~A : ~A ~A, diff ~A" frame-id p viewing (norm (send p :difference-rotation viewing))))
      ))
  )

(run-all-tests)
(exit)
